using BepuUtilities;
using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;
using static BepuUtilities.GatherScatter;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.Constraints
{
    /// <summary>
    /// 约束连接四个实体中心的四面体的体积以匹配目标体积。
    /// 从(Ab X Ac)*ad计算的缩放体积;根据四面体的缠绕,体积可能为负值。
    /// </summary>
    public struct VolumeConstraint : IFourBodyConstraintDescription<VolumeConstraint>
    {
        /// <summary>
        /// 是四面体目标体积的6倍。从(Ab X Ac)*ad计算;根据四面体的缠绕情况,这可能是负数。
        /// </summary>
        public float TargetScaledVolume;
        /// <summary>
        /// 弹簧频率和阻尼参数。
        /// </summary>
        public SpringSettings SpringSettings;

        /// <summary>
        /// 创建新的体积约束,使用一组初始位置初始化目标体积。
        /// </summary>
        /// <param name="a">第一个正文的初始位置。</param>
        /// <param name="b">第二个正文的初始位置。</param>
        /// <param name="c">第三个主体的初始位置。</param>
        /// <param name="d">第四个正文的初始位置。</param>
        /// <param name="springSettings">要应用于体积约束的Spring设置。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public VolumeConstraint(in Vector3 a, in Vector3 b, in Vector3 c, in Vector3 d, SpringSettings springSettings)
        {
            TargetScaledVolume = Vector3.Dot(Vector3.Cross(b - a, c - a), d - a);
            SpringSettings = springSettings;
        }

        public int ConstraintTypeId
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                return VolumeConstraintTypeProcessor.BatchTypeId;
            }
        }

        public Type TypeProcessorType => typeof(VolumeConstraintTypeProcessor);

        public void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
        {
            ConstraintChecker.AssertValid(SpringSettings, nameof(VolumeConstraint));
            Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type.");
            ref var target = ref GetOffsetInstance(ref Buffer<VolumeConstraintPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
            Unsafe.As<Vector<float>, float>(ref target.TargetScaledVolume) = TargetScaledVolume;
            SpringSettingsWide.WriteFirst(SpringSettings, ref target.SpringSettings);
        }

        public void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out VolumeConstraint description)
        {
            Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type.");
            ref var source = ref GetOffsetInstance(ref Buffer<VolumeConstraintPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
            description.TargetScaledVolume = Unsafe.As<Vector<float>, float>(ref source.TargetScaledVolume);
            SpringSettingsWide.ReadFirst(source.SpringSettings, out description.SpringSettings);
        }
    }

    public struct VolumeConstraintPrestepData
    {
        public Vector<float> TargetScaledVolume;
        public SpringSettingsWide SpringSettings;
    }

    public struct VolumeConstraintProjection
    {
        public Vector3Wide JacobianB;
        public Vector3Wide JacobianC;
        public Vector3Wide JacobianD;
        public Vector<float> EffectiveMass;
        public Vector<float> BiasImpulse;
        public Vector<float> SoftnessImpulseScale;
        public Vector<float> InverseMassA;
        public Vector<float> InverseMassB;
        public Vector<float> InverseMassC;
        public Vector<float> InverseMassD;
    }

    public struct VolumeConstraintFunctions : IFourBodyConstraintFunctions<VolumeConstraintPrestepData, VolumeConstraintProjection, Vector<float>>
    {
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Prestep(Bodies bodies, ref FourBodyReferences bodyReferences, int count, float dt, float inverseDt,
            ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref BodyInertias inertiaC, ref BodyInertias inertiaD,
            ref VolumeConstraintPrestepData prestep, out VolumeConstraintProjection projection)
        {
            bodies.GatherOffsets(ref bodyReferences, count, out var ab, out var ac, out var ad);

            // 具有顶点a、b、c、d的平行六面体的体积为：
            // (AB X Ac)*ad
            // 具有相同边的四面体的体积是这个体积的六分之一。作为一个恒定的因素,它是不相关的。所以约束条件就是：
            // OriginalVolume*6=(Ab X Ac)*ad
            // 求导数以获得速度约束：
            // 0=d/dt(Ab X Ac)*ad+(Ab X Ac)*d/dt(Ad)
            // 0=(d/dt(Ab)x ac+ab x d/dt(Ac))*ad+(Ab X Ac)*d/dt(Ad)
            // 0=(d/dt(Ab)x ac)*ad+(ab x d/dt(Ac))*ad+(Ab X Ac)*d/dt(Ad)
            // 0=(Ac X Ad)*d/dt(Ab)+(Ad X Ab)*d/dt(Ac)+(Ab X Ac)*d/dt(Ad)
            // 给出线性雅克比：
            // JA：-ac x ad-ad x ab-ab x ac
            // JB：AC x广告
            // JC：AD x ab
            // JD：AB x AC
            // JA可以用一些代数压缩成类似于其他Jacobian的形式,但没有必要,因为只执行几个减法很便宜。
            // 请注意,我们也不存储A的雅可比。A的雅可比矩阵可以从B、C和D廉价地找到。
            // 我们也不会将雅可比混合到有效质量或反质量中-尽管这会节省ALU时间,但这里的目标是将内存带宽降至最低
            // 往往是任何多线程模拟的瓶颈。(尽管是1自由度约束,但这不需要输出反惯性张量,因此预乘不是成功的。)

            Vector3Wide.CrossWithoutOverlap(ac, ad, out projection.JacobianB);
            Vector3Wide.CrossWithoutOverlap(ad, ab, out projection.JacobianC);
            Vector3Wide.CrossWithoutOverlap(ab, ac, out projection.JacobianD);
            Vector3Wide.Add(projection.JacobianB, projection.JacobianC, out var negatedJA);
            Vector3Wide.Add(projection.JacobianD, negatedJA, out negatedJA);

            Vector3Wide.Dot(negatedJA, negatedJA, out var contributionA);
            Vector3Wide.Dot(projection.JacobianB, projection.JacobianB, out var contributionB);
            Vector3Wide.Dot(projection.JacobianC, projection.JacobianC, out var contributionC);
            Vector3Wide.Dot(projection.JacobianD, projection.JacobianD, out var contributionD);

            // 通过填充雅可比贡献来防止奇异性。这在很大程度上是一种黑客行为,但也是一种相当简单的黑客行为。
            // 与尝试保护InverseEffectiveMass值本身相比,它对调整的敏感度较低,因为它对比例和质量都很敏感。

            // 根据目标卷选择epsilon。请注意,体积~=宽度^3,而我们的雅可比贡献类似于(Ac X Ad)*(Ac X Ad),它是成比例的
            // 到三角形面积的ACD的平方。换句话说,贡献是~Width^4。
            // 按恒定因子缩放体积将不会与雅可比贡献的增长率相匹配。
            // 我们将忽略这一点,直到证明这是一个明显的问题,因为Vector<T>不公开exp或power,而且这很便宜。
            // 还可以实施,但价值不是很高。
            var epsilon = 5e-4f * prestep.TargetScaledVolume;
            contributionA = Vector.Max(epsilon, contributionA);
            contributionB = Vector.Max(epsilon, contributionB);
            contributionC = Vector.Max(epsilon, contributionC);
            contributionD = Vector.Max(epsilon, contributionD);
            var inverseEffectiveMass = contributionA * inertiaA.InverseMass + contributionB * inertiaB.InverseMass + contributionC * inertiaC.InverseMass + contributionD * inertiaD.InverseMass;
            projection.InverseMassA = inertiaA.InverseMass;
            projection.InverseMassB = inertiaB.InverseMass;
            projection.InverseMassC = inertiaC.InverseMass;
            projection.InverseMassD = inertiaD.InverseMass;

            SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale);

            projection.EffectiveMass = effectiveMassCFMScale / inverseEffectiveMass;
            // 计算位置误差和偏置速度。计算误差时请注意减法的顺序-我们希望偏置速度抵消分离。
            Vector3Wide.Dot(projection.JacobianD, ad, out var unscaledVolume);
            projection.BiasImpulse = (prestep.TargetScaledVolume - unscaledVolume) * (1f / 6f) * positionErrorToVelocity * projection.EffectiveMass;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref BodyVelocities velocityD,
            ref VolumeConstraintProjection projection, ref Vector3Wide negatedJacobianA, ref Vector<float> impulse)
        {
            Vector3Wide.Scale(negatedJacobianA, projection.InverseMassA * impulse, out var negativeVelocityChangeA);
            Vector3Wide.Scale(projection.JacobianB, projection.InverseMassB * impulse, out var velocityChangeB);
            Vector3Wide.Scale(projection.JacobianC, projection.InverseMassC * impulse, out var velocityChangeC);
            Vector3Wide.Scale(projection.JacobianD, projection.InverseMassD * impulse, out var velocityChangeD);
            Vector3Wide.Subtract(velocityA.Linear, negativeVelocityChangeA, out velocityA.Linear);
            Vector3Wide.Add(velocityB.Linear, velocityChangeB, out velocityB.Linear);
            Vector3Wide.Add(velocityC.Linear, velocityChangeC, out velocityC.Linear);
            Vector3Wide.Add(velocityD.Linear, velocityChangeD, out velocityD.Linear);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static void GetNegatedJacobianA(in VolumeConstraintProjection projection, out Vector3Wide jacobianA)
        {
            Vector3Wide.Add(projection.JacobianB, projection.JacobianC, out jacobianA);
            Vector3Wide.Add(projection.JacobianD, jacobianA, out jacobianA);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref BodyVelocities velocityD, ref VolumeConstraintProjection projection, ref Vector<float> accumulatedImpulse)
        {
            // 与大多数约束不同的是,在某些情况下,体积约束中的Jacobian可能会大幅更改大小和方向。
            // 重复使用前一帧的累积脉冲可能会导致灾难性的错误猜测,需要多次迭代才能更正。
            // 取而代之的是,就目前而言,我们只需清除积累的冲动。正因为如此,在持续的作用力期间,约束会稍微柔和一些,但它有助于避免
            // 在最坏的情况下,爆炸和轻微的软化通常不是体积限制的大问题。
            // TODO：这是一种相当粗糙的方法,因为我们已经加载了速度,尽管没有对它们做任何事情。
            // 有两个选项：通过更新累积脉冲以响应雅可比的变化来修复潜在问题,或者在特殊情况下完全不加载速度。
            accumulatedImpulse = default;
            // 真正温暖的开始应该是这样的：
            // GetNegedJacobianA(投影,Out Var Negated JacobianA);
            // 施加脉冲(参考速度A、参考速度B、参考速度C、参考速度D、参考投影、参考否定雅可比A、参考累积脉冲);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref BodyVelocities velocityD, ref VolumeConstraintProjection projection, ref Vector<float> accumulatedImpulse)
        {
            // CSI=投射.BiasImPulse-累加脉冲*投射.SoftnessImPulseScale-(csia线性+csiaAngular+csib线性+csibAngular);
            GetNegatedJacobianA(projection, out var negatedJacobianA);
            Vector3Wide.Dot(negatedJacobianA, velocityA.Linear, out var negatedContributionA);
            Vector3Wide.Dot(projection.JacobianB, velocityB.Linear, out var contributionB);
            Vector3Wide.Dot(projection.JacobianC, velocityC.Linear, out var contributionC);
            Vector3Wide.Dot(projection.JacobianD, velocityD.Linear, out var contributionD);
            var csv = contributionB + contributionC + contributionD - negatedContributionA;
            var csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csv * projection.EffectiveMass;
            accumulatedImpulse += csi;

            ApplyImpulse(ref velocityA, ref velocityB, ref velocityC, ref velocityD, ref projection, ref negatedJacobianA, ref csi);
        }

    }


    /// <summary>
    /// 处理一组体积约束的解算迭代。
    /// </summary>
    public class VolumeConstraintTypeProcessor : FourBodyTypeProcessor<VolumeConstraintPrestepData, VolumeConstraintProjection, Vector<float>, VolumeConstraintFunctions>
    {
        public const int BatchTypeId = 32;
    }
}
